clear all;clc;
%   MUMSA IOA WYLin 
X=[79.51606800000	65.42877100000	80.00000000	54.72180000
9.72397300000	8.01638700000	8.04566200	8.53201000
45.84252400000	47.22165500000	50.81902000	31.48480000
51.43284800000	44.13656000000	42.20801000	33.21310000
8.21392200000	-11.57085800000	-10.63697000	0.42306100
-2.95395750000	-1.90491400000	-2.29109000	5.99960000
2.02110900000	10.63541400000	8.49481300	7.30574000
13.21658780000	-1.67547700000	-0.75796780	4.88892000
5.59694450000	3.86733000000	3.88921000	0.09651430
0.63768730000	2.41993100000	2.44943700	0.00000000
1.32553290000	3.10926700000	3.15396900	0.71464300
2.00803390000	3.81295000000	3.83711400	1.42348000
2.69556590000	4.50644000000	4.52017100	2.12970000
3.38457940000	5.18113900000	5.20479900	2.80325000
4.08293762000	5.88342000000	5.89853600	3.47410000
4.79845482000	0.29626300000	0.31620400	4.14885000
5.51170565000	0.99115300000	1.02355500	4.85494000
6.21279190000	1.70778700000	1.73899200	5.56444000
0.63718660000	2.41886500000	2.44943700	6.28318000

]
X=round(X,10)
for jj=1:size(X,2)
x=X(:,jj)% 
C1=[20;10];
C2=[17.66;15.142];
C3=[11.736;17.878];
C4=[5;16.928];
C5=[0.60307;12.736];
C6=[0.60307;7.2638];
C7=[5;3.0718];
C8=[11.736;2.1215];
C9=[17.66;4.8577];
C10=[20;10];
% Quy dao cho truoc
CD=[C1,C2,C3,C4,C5,C6,C7,C8,C9,C10];
% ==========================
Cdx=CD(1,:); Cdy=CD(2,:);
r1=x(1);
r2=x(2);
r3=x(3);
r4=x(4);
rcx=x(5);
rcy=x(6);
f=0;
for i=1:length(Cdx)
     teta2=x(9+i);
    K1=r1/r2;
    K4=r1/r3;
    K5=(r4^2-r1^2-r2^2-r3^2)/(2*r3*r2);
    D=cos(teta2)-K1+K4*cos(teta2)+K5;
    E=-2*sin(teta2);
    F=K1+(K4-1)*cos(teta2)+K5;
    ATAN3=(-E-sqrt(E^2-4*D*F))/(2*D);
    teta3=real(2*atan(ATAN3));
    Crx=r2*cos(teta2)+rcx*cos(teta3)-rcy*sin(teta3);
    Cry=r2*sin(teta2)+rcx*sin(teta3)+rcy*cos(teta3);
    C(:,i)=[cos(x(9)),-sin(x(9));sin(x(9)),cos(x(9))]*[Crx;Cry]+[x(7);x(8)];
    f=f+sum((C(:,i)-[Cdx(i);Cdy(i)]).^2);      
end
xx=[x;f];
DATA(:,jj)=xx;
end